Towards Optimal Synthesis of Reactive Ouput-Feedback Controllers¶
Motivation¶
- Would be excellent if we can automatically synthesize output feedback controllers, which we could guarantee are optimal according to some metric.
- Would like to be able to verify these controllers via the safety verification method (or something close to that) described in Barry, Majumdar, Tedrake (2012).
- (This work provided a great tool to check a given controller. Left controller synthesis to future work.)
- For "quads-in-the-real-world" (outside, at respectable speeds, like in FLA), there may be an increasing utility of reactive controllers, or at least ones with planning horizons actually limited by real sensors + occlusions
Approach so far¶
- Reason about dynamics of pixels
- Parameterize all possible obstacle environments via a set of basis functions
- Use ODE approximation of pixels PDE
- Optimization over possible controllers (starting just with linear combination)
Consider a known function $s$ which maps from $[-\frac{\theta_{FOV}}{2},\frac{\theta_{FOV}}{2}]$ to the raycasted distance to an obstacle, where $S_{max}$ is the moving maximum sensing horizon of the sensor.
$$s: [-\frac{\theta_{FOV}}{2},\frac{\theta_{FOV}}{2}] \rightarrow [0,S_{max}] $$The function $s$ clearly has discontinuities (should be piecewise continuous), but we might be able to approximate $s$ as continuous. (Would let us treat as not a hybrid system).
2D Dubin's Plane¶
theta_slider = widgets.FloatSlider(min=-90, max=90, step=1, value=0)
w=widgets.interactive(plotPlane,theta_deg=theta_slider)
display(w)
As a special case, we first focus on a sensor that has a 360 degree field of view (FOV), and we can later consider the problem of having obstacles move in and out of sensor FOV:
$$s: [-\pi,\pi] \rightarrow [0,S_{max}] $$RotatingBot¶
import os
import io
import base64
from IPython.display import HTML
video = io.open('/Users/pflomacpro/GeometricPDEs/_RotatingBot/bot_h264.mp4', 'r+b').read()
encoded = base64.b64encode(video)
HTML(data='''<video alt="test" controls>
<source src="data:video/mp4;base64,{0}" type="video/mp4" />
</video>'''.format(encoded.decode('ascii')))
What we need to find is a geometric PDE that looks something like:
$$ \frac{\partial}{\partial t} s(\theta) = f(s,\psi, \dot{\psi})$$And in our Dubin's car model (assume constant linear velocity too) we can directly control $\dot{\psi} = u$ and would like to do so as a function of $s$ only. ($u=u(s)$):
$$ \frac{\partial}{\partial t} s(\theta) = f(s,\psi, u(s))$$Where $\psi$ is the yaw of the UAV and $u$ is some control input.
The control input $u$ may be calculated via a reactive controller, for example of the form:
$$ u = k_1\int w_1(\theta)s(\theta)d\theta + k_2\int w_s(\theta)s(\theta)d\theta$$Barry, Majumdar, Tedrake 2012 can then tell us where the controller is safe, and where it's not.
Getting to an ODE for pixel dynamics¶
What we would really like to do is, given a finite dimensional approximation of all possible obstacle environments, be able to search for the optimal output feedback controller that applies to all possible environments.
Hence we need to use a finite dimensional space to approximate the function $s$.
Given some vector of basis functions $\mathbf{w}$ and vector of weights $\mathbf{c}$ we can approximate $s$ as:
$$ s(\theta) \approx c_1w_1(\theta) + c_2w_2(\theta) + ... + c_Nw_n(\theta) $$Which then gives us a PDE that approximates $f$:
$$ \frac{\partial}{\partial t} s(\theta) \approx \dot{c}_1w_1(\theta) + \dot{c}_2w_2(\theta) + ... + \dot{c}_Nw_N(\theta) $$$$ \approx f(s,\psi, \dot{\psi})$$Let us define that:
$$ s_{approx}(\theta) = c_1w_1(\theta) + c_2w_2(\theta) + ... + c_Nw_n(\theta) $$$$ \frac{\partial}{\partial t} s_{approx}(\theta) = \dot{c}_1w_1(\theta) + \dot{c}_2w_2(\theta) + ... + \dot{c}_Nw_N(\theta) $$We can now instead think of $f$ as a vector $\mathbf{f}$ which is a function $\mathbf{f}(c_0,c_1,...,c_N,\theta,\dot{\psi)}$, and whose elements are $f_1, f_2, ..., f_N$. Also write $\mathbf{c}$ as the vector $\mathbf{c} = (c_1,c_2,...,c_N)$.
We now have an ODE for the weights:
$$ \frac{d}{d t} \mathbf{c} = \begin{pmatrix} f_1(\mathbf{c}, \dot{\psi}) \\ f_2(\mathbf{c}, \dot{\psi}) \\ \vdots \\ f_N(\mathbf{c}, \dot{\psi}) \\ \end{pmatrix} $$PDE for RotatingBot¶
Believe that our PDE is:
$$ \frac{\partial s}{\partial t} = \frac{\partial s}{\partial \theta} \dot{\psi} $$Where $s(\theta,t)$ is the distance to an obstacle at time $t$ and angle $\theta$, and $\dot{\psi}$ is the rate of rotation of the vehicle. If we assume that we can directly control the rate of change, then $u = \dot{\psi}$ and we have:
$$ \frac{\partial s}{\partial t} = \frac{\partial s}{\partial \theta} u $$Controller synthesis¶
Want to maximize some property of $\mathbf{c} \in \mathbb{R}^n$ (where $n = $ # of basis functions) that corresponds to the size of the safety-verified set of conditions $\mathbf{c}_{t=0}$.
The easiest is to imagine a linear combination:
$$u=\mathbf{K} \cdot \mathbf{c} $$.
Where $\mathbf{K}^{1 \times n}, \mathbf{c}^{n \times 1}$. We can either hand-design $K$ or we can search for a $K$ that maximizes the safety-verified set over $\mathbf{c}$.
There are a few different ideas of what property to maximize.
- Maximize volume. For this, we may need to bound $\mathbf{c}_{t=0}$ as $\{\mathbf{c}_{t=0} \in [0, S_{max}] \subset \mathbb{R}^n\}$ (Ani says not known how to do with SOS).
- Maximize the minimum $c_{min}$. For this, we may want to choose only a subset $c_{i:j}$ where $i,...,j$ represent the coefficients of the basis functions in the middle in front of us, only the ones that we care about not hitting. (Ani says may be possible with SOS).
- Maximize some sphere around a point in $R^n$. May again only want to think about $R^k$ where $k = j - i$. (Ani says can do with SOS).
Each element of $\mathbf{K}$ would be a decision variable our optimization.
Choice of basis functions¶
A polynomial basis would naturally lead to SOS verification. It might not be easy, however, to know how to "stay away" from the weights $\textbf{c}$ of polynomial bases. (It is not clear how each $c_i$ should indicate where obstacles are).
Some options to consider:
- polynomials
- trig polynomials
- linear combinations of Gaussians
- inverse quadratic $\phi(r) = \frac{1}{1 + (\epsilon r)^2} $
Set of basis functions needs to be:
- closed under addition
- closed under scaling
- closed under multiplication
Periodic over 2$\pi$¶
Maybe not necessary, but probably ideal
Currently trying Fourier series approximations to a linear combination of three Gaussians:
A trick is that it is better to use a linear combination of three Gaussians separated by $2\pi$, so then the periodicity is naturally imposed.
Need to pick the degree of the Fourier series approximation, and this fit is dependent on the sharpness (variance) of the chosen Gaussians.
Note that in this Fourier formulation, there are two "layers" of weights. For each basis function, there are the Fourier weights (numbering $2M$, where $M$ is the degree of the approximation) and this is collectively considered a basis function.... are two eighting of the entire basis function becomes an element in $\mathbf{c}$
Creating basis functions¶
import math
from sympy.mpmath import *
I = [-math.pi, math.pi]
f = lambda x: 2.71 ** (-(x-0)**2 / (2 * 1**2))
x = np.linspace(-10,10,1000)
y = f(x)
plt.plot(x,y)
plt.show()
M = 6
for i in range(1,M+1):
print "--------"
print "m = " + str(i)
cs = fourier(f, I, i)
nprint(cs[0])
nprint(cs[1])
plot([f, lambda x: fourierval(cs, I, x)], I)
plt.show()
# Approximation of just one Gaussian over the range [-pi,pi]
from sympy.mpmath import *
I = [-math.pi, math.pi]
N = 10
M=3
for i in np.linspace(-3.14,3.14,10):
print i
f = lambda x: 2.71 ** (-(x-i)**2)
x = np.linspace(-10,10,1000)
y = f(x)
plt.plot(x,y)
plt.show()
cs = fourier(f, I, M)
nprint(cs[0])
nprint(cs[1])
plot([f, lambda x: fourierval(cs, I, x)], I)
plt.show()
# Approximation of linear combination of three Gaussians (separated by 2pi) over the range [-pi,pi]
from sympy.mpmath import *
I = [-math.pi, math.pi]
N = 12 # number of Fourier-approximation-of-3-Gaussian basis functions
M = 3 # order of Fourier series
for i in np.linspace(-3.14,3.14-3.14/11,N):
print i
f = lambda x: 2.71 ** (-(x-i)**2) + 2.71 ** (-(x-i-2*math.pi)**2) + 2.71 ** (-(x-i+2*math.pi)**2)
x = np.linspace(-10,10,1000)
y = f(x)
plt.plot(x,y)
plt.show()
cs = fourier(f, I, M)
nprint(cs[0])
nprint(cs[1])
plot([f, lambda x: fourierval(cs, I, x)], I)
plt.show()
# Approximation of linear combination of three Gaussians (separated by 2pi) over the range [-pi,pi]
# Decreasing variance
from sympy.mpmath import *
I = [-math.pi, math.pi]
N = 12 # number of Fourier-approximation-of-3-Gaussian basis functions
M = 6 # order of Fourier series
sig = 0.4
n = 0
for i in np.linspace(-3.14,3.14-3.14/11,N):
n = n + 1
print i
f = lambda x: 2.71 ** (-(x-i)**2 / (2*sig**2)) + 2.71 ** (-(x-i-2*math.pi)**2 / (2*sig**2)) + 2.71 ** (-(x-i+2*math.pi)**2 / (2*sig**2))
x = np.linspace(-10,10,1000)
y = f(x)
plt.plot(x,y)
plt.show()
cs = fourier(f, I, M)
nprint(cs[0])
nprint(cs[1])
plot([f, lambda x: fourierval(cs, I, x)], I)
plt.show()
for i in np.linspace(-3.14,3.14-3.14/11,N):
print i
f = lambda x: 2.71 ** (-(x-i)**2 / (2*sig**2)) + 2.71 ** (-(x-i-2*math.pi)**2 / (2*sig**2)) + 2.71 ** (-(x-i+2*math.pi)**2 / (2*sig**2))
x = np.linspace(-10,10,1000)
y = f(x)
plt.plot(x,y)
plt.show()
cslist = []
M=6
for i in np.linspace(-3.14,3.14-3.14/11,N):
print i
f = lambda x: 2.71 ** (-(x-i)**2 / (2*sig**2)) + 2.71 ** (-(x-i-2*math.pi)**2 / (2*sig**2)) + 2.71 ** (-(x-i+2*math.pi)**2 / (2*sig**2))
x = np.linspace(-10,10,1000)
y = f(x)
cs = fourier(f, I, M)
cslist.append(cs)
for i in range(len(cslist)):
cs = cslist[i]
def f_basis_practice(x):
b = math.pi
a = -math.pi
m = 2.0*math.pi / (b - a)
y = 0
M = 6
for k in range(M+1):
y = y + cs[0][k] * math.cos(k * m * x)
y = y + cs[1][k] * math.sin(k * m * x)
return y
x = np.linspace(-math.pi,math.pi,1000)
y = x * 0.0
for i in range(len(x)):
y[i] = f_basis_practice(x[i])
plt.plot(x,y)
plt.show()
Fitting of basis functions to obstacle environment data¶
Started with least squares so far:
$$ \hat{\mathbf{c}} = (\Theta^T\Theta)^{-1}\Theta^T\textbf{s} $$Where we have $n$ points $(\theta_i, s_i)$, and are fitting with coefficients $c_1, ..., c_N$ for our $N$ basis functions $\phi_1, ..., \phi_N$:
$$\begin{bmatrix}s_1 \\s_2 \\ \vdots \\ s_n\end{bmatrix} = \begin{bmatrix}\phi_1(\theta_1) & \phi_2(\theta_1) & \phi_3(\theta_1) & ... & \phi_N(\theta_1) \\ \phi_1(\theta_2) & \phi_2(\theta_2) & \phi_3(\theta_2) & ... & \phi_N(\theta_2) \\ \vdots & \vdots & \vdots & \ddots & \vdots \\ \phi_1(\theta_n) & \phi_2(\theta_n) & \phi_3(\theta_n) & ... & \phi_N(\theta_n) \\ \end{bmatrix} \begin{bmatrix}c_1 \\ c_2 \\ \vdots \\ c_N\end{bmatrix}$$$$\textbf{s} = \Theta \mathbf{c}$$import csv
o = open('laserdata.csv', 'rU')
mydata = csv.reader(o)
laseAngles2 = []
laserDepths = []
for row in mydata:
laseAngles2.append(float(row[0]))
laserDepths.append(float(row[1]))
#%pylab inline
plt.plot(laseAngles2,laserDepths,'.')
plt.xlabel(r'$\theta_{plane coord}$')
plt.ylabel(r'$s(\theta)$')
plt.axis([-4, 4, 0, 15])
plt.show()
# Approximation of linear combination of three Gaussians (separated by 2pi) over the range [-pi,pi]
from sympy.mpmath import *
I = [-math.pi, math.pi]
N = 12 # number of Fourier-approximation-of-3-Gaussian basis functions
M = 6 # order of Fourier series
sig = 0.4
import os
import io
import base64
from IPython.display import HTML
video = io.open('/Users/pflomacpro/GeometricPDEs/_RotatingBot/bot_h264.mp4', 'r+b').read()
encoded = base64.b64encode(video)
HTML(data='''<video alt="test" controls>
<source src="data:video/mp4;base64,{0}" type="video/mp4" />
</video>'''.format(encoded.decode('ascii')))
import numpy as np
import matplotlib.pyplot as plt
import scipy.optimize as opt
from linear_regression import LinearRegression
from gradient_descent import GradientDescent, quad, quadGrad
%matplotlib inline
def plot(lr,w):
# plot sin(2*phi*x) in green
x = np.linspace(-math.pi+0.2,math.pi-0.2,1000)
lr_temp = LinearRegression(x,x,lr.numFeatures-1)
reg_prediction = np.dot(lr_temp.phi,w)
plt.plot(x, reg_prediction, color='r')
plt.scatter(lr.x, lr.y, color='b', marker='o',facecolors='none')
plt.xlabel('x')
plt.ylabel('y')
plt.show()
def plotRBFbases(lr,w):
# plot sin(2*phi*x) in green
x = np.linspace(-math.pi+0.2,math.pi-0.2,1000)
reg_prediction = w[0]*lr.bases[0](x) + w[1]*lr.bases[1](x) + w[2]*lr.bases[2](x) + w[3]*lr.bases[3](x) + \
w[4]*lr.bases[4](x) + w[5]*lr.bases[5](x) + w[6]*lr.bases[6](x) + w[7]*lr.bases[7](x) + \
w[8]*lr.bases[8](x) + w[9]*lr.bases[9](x) + w[10]*lr.bases[10](x) + w[11]*lr.bases[11](x)
plt.plot(x, reg_prediction, color='r')
plt.scatter(lr.x, lr.y, color='b', marker='o',facecolors='none')
plt.xlabel('x')
plt.ylabel('y')
plt.show()
M = 11
lr = LinearRegression(laseAngles2,laserDepths,M)
w = lr.reg()
w_initial = 0*np.zeros((1,12))
print "regression weights for M = " + str(M)
print w
plot(lr,w)
N = 12
lr = LinearRegression(laseAngles2,laserDepths,11)
lr.setBases(N,cslist)
lr.RBF_Phi(N)
w = lr.reg()
print "regression weights for M = " + str(M)
print w
plotRBFbases(lr,w)
import csv
o = open('laserdata.csv', 'rU')
mydata = csv.reader(o)
laseAngles2 = []
laserDepths = []
for row in mydata:
laseAngles2.append(float(row[0]))
laserDepths.append(float(row[1]))
#%pylab inline
plt.plot(laseAngles2,laserDepths,'.')
plt.xlabel(r'$\theta_{plane coord}$')
plt.ylabel(r'$s(\theta)$')
plt.axis([-4, 4, 0, 15])
plt.show()
# Approximation of linear combination of three Gaussians (separated by 2pi) over the range [-pi,pi]
from sympy.mpmath import *
I = [-math.pi, math.pi]
N = 12 # number of Fourier-approximation-of-3-Gaussian basis functions
M = 6 # order of Fourier series
sig = 0.4
import numpy as np
import matplotlib.pyplot as plt
import scipy.optimize as opt
from linear_regression import LinearRegression
from gradient_descent import GradientDescent, quad, quadGrad
%matplotlib inline
def plot(lr,w):
# plot sin(2*phi*x) in green
x = np.linspace(-math.pi+0.2,math.pi-0.2,1000)
lr_temp = LinearRegression(x,x,lr.numFeatures-1)
reg_prediction = np.dot(lr_temp.phi,w)
plt.plot(x, reg_prediction, color='r')
plt.scatter(lr.x, lr.y, color='b', marker='o',facecolors='none')
plt.xlabel('x')
plt.ylabel('y')
plt.show()
def plotRBFbases(lr,w):
# plot sin(2*phi*x) in green
x = np.linspace(-math.pi+0.2,math.pi-0.2,1000)
reg_prediction = w[0]*lr.bases[0](x) + w[1]*lr.bases[1](x) + w[2]*lr.bases[2](x) + w[3]*lr.bases[3](x) + \
w[4]*lr.bases[4](x) + w[5]*lr.bases[5](x) + w[6]*lr.bases[6](x) + w[7]*lr.bases[7](x) + \
w[8]*lr.bases[8](x) + w[9]*lr.bases[9](x) + w[10]*lr.bases[10](x) + w[11]*lr.bases[11](x)
plt.plot(x, reg_prediction, color='r')
plt.scatter(lr.x, lr.y, color='b', marker='o',facecolors='none')
plt.xlabel('x')
plt.ylabel('y')
plt.show()
M = 11
lr = LinearRegression(laseAngles2,laserDepths,M)
w = lr.reg()
w_initial = 0*np.zeros((1,12))
print "regression weights for M = " + str(M)
print w
plot(lr,w)
N = 12
lr = LinearRegression(laseAngles2,laserDepths,11)
lr.setBases(N,cslist)
lr.RBF_Phi(N)
w = lr.reg()
print "regression weights for M = " + str(M)
print w
plotRBFbases(lr,w)
import os
import io
import base64
from IPython.display import HTML
video = io.open('/Users/pflomacpro/GeometricPDEs/_RotatingBot/bot_h264.mp4', 'r+b').read()
encoded = base64.b64encode(video)
HTML(data='''<video alt="test" controls>
<source src="data:video/mp4;base64,{0}" type="video/mp4" />
</video>'''.format(encoded.decode('ascii')))
Image space vs. 2D¶
What is nice about thinking in image space is that we just define distances to obstacles as a function of one variable.
If we think in 2D, then it is more similar to the signed distance function representation.